//
// Created by whitby on 8/22/23.
//

#include "System.h"
#include "Sensor/Camera.h"
#include "ORB/ORBVocabulary.h"

#include <unistd.h>
#include <iostream>
#include <iomanip>
#include <utility>

using namespace std;

namespace mono_orb_slam2 {

    System::System(const std::string &settingYaml, const std::string &vocabularyFile, bool useViewer) : be_reset(false) {
        cv::FileStorage fs(settingYaml, cv::FileStorage::READ);
        if (!fs.isOpened()) {
            cout << "fail to load " << settingYaml << endl;
            terminate();
        }

        // orb vocabulary
        cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
        bool loaded = ORBVocabulary::createORBVocabulary(vocabularyFile);
        if (!loaded) {
            cerr << "Wrong path to vocabulary." << endl;
            cerr << "Fail to open at " << vocabularyFile << endl;
            exit(-1);
        }
        cout << "Vocabulary loaded!" << endl << endl;

        // camera
        Camera::create(fs["Camera"]);
        const Camera *camera_ptr = Camera::getCamera();
        camera_ptr->print();

        // map
        point_map = new Map();

        // threads
        tracker = new Tracking(fs["ORB"], point_map, this);
        tracking_state = tracker->state;

        local_mapper = new LocalMapping(point_map);
        local_mapping = new thread(&LocalMapping::Run, local_mapper);
        tracker->setLocalMapper(local_mapper);

        if (useViewer) {
            cv::FileNode viewNode = fs["View"];
            frame_drawer = new FrameDrawer(camera_ptr->width, camera_ptr->height);
            map_drawer = new MapDrawer(point_map, viewNode);

            viewer = new Viewer(this, frame_drawer, map_drawer, fs["Camera"], viewNode);
            viewing = new thread(&Viewer::Run, viewer);
            tracker->setViewer(viewer);
        }
    }

    System::~System() {
        local_mapping->join();
        if (viewing != nullptr) viewing->join();
        delete local_mapping;
        delete viewing;
        delete tracker;
        delete local_mapper;
        delete point_map;
        delete viewer;
        delete frame_drawer;
        delete map_drawer;
        Camera::destroy();
    }

    void System::Track(double timeStamp, cv::Mat grayImg) {
        // check reset
        {
            lock_guard<mutex> lock(reset_mutex);
            if (be_reset) {
                tracker->reset();
                be_reset = false;
            }
        }

        tracker->Track(timeStamp, std::move(grayImg));
        {
            lock_guard<mutex> lock(state_mutex);
            tracking_state = tracker->state;
        }
    }

    void System::Reset() {
        lock_guard<mutex> lock(reset_mutex);
        be_reset = true;
    }

    void System::ShutDown() {
        local_mapper->requestFinish();

        if (viewer != nullptr) {
            viewer->requestFinish();
            while (!viewer->isFinished())
                usleep(5000);
        }

        while (!local_mapper->isFinish()) {
            usleep(5000);
        }

        if (viewer != nullptr)
            pangolin::BindToContext("mono-orb-slam2: map viewer");
    }

    void System::saveKeyFrameTrajectory(const std::string &fileName) {
        cout << endl << "saving keyframe trajectory to " << fileName << "..." << endl;

        vector<shared_ptr<KeyFrame>> keyFrames = point_map->getAllKeyFrames();
        sort(keyFrames.begin(), keyFrames.end(),
             [](const shared_ptr<KeyFrame> &kf1, const shared_ptr<KeyFrame> &kf2) { return kf1->id < kf2->id; });

        ofstream outFile(fileName);
        outFile << setiosflags(ios::fixed) << setprecision(6);
        for (const auto &kf: keyFrames) {
            const Pose Tcw = kf->getPose();
            const Eigen::Vector3f twc = kf->getCameraCenter();
            const Eigen::Quaternionf q(Tcw.R.transpose());

            outFile << kf->frame_id << " " << kf->timestamp << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w()
                    << " " << twc.x() << " " << twc.y() << " " << twc.z() << endl;
        }
        outFile.close();
        cout << "trajectory saved!" << endl;
    }

    int System::getTrackingState() {
        lock_guard<mutex> lock(state_mutex);
        return tracking_state;
    }

} // mono_orb_slam2